#include "RGBDFrame.h"

#include <string>
#include <boost/lexical_cast.hpp>
#include <pcl/features/normal_3d.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
//#include "InformationPresentation.cpp"
//#include "myalgebra.cpp"
//#include "constants.h"

using namespace std;


RGBDFrame::RGBDFrame()
{
}

void RGBDFrame::print()
{
	printf("Camera: %ld\n",(unsigned long)camera);
	printf("id_number: %i\n",id_number);
	printf("rgb_path: %s\n",rgb_path.c_str());
	printf("depth_path: %s\n",depth_path.c_str());
	printf("timing: %f\n",timing);
}

RGBDFrame::RGBDFrame(RGBDCamera * used_camera)
{

	camera = used_camera;
	id_number = camera -> current_frame_number;
	//printf("id number: %i\n",id_number);
	rgb_path = *camera->rgb_list[camera->current_frame_number];
	depth_path = *camera->depth_list[camera->current_frame_number];
	timing = camera->timings[camera->current_frame_number];
	keypoints = 0;
	setupKeyPointData();
	setupImageDescriptor();//Can probably cut 0.01s of here
	//exit(0);
	printf("Getting frame...\n");
	showFrame();
}

RGBDFrame::~RGBDFrame(){
	delete image_descriptor;
	delete keypoints;
	printf("~RGBDFrame(): stopping\n");
	exit(0);
}

void RGBDFrame::store(string path)
{
	printf("RGBDFrame store: %s\n",path.c_str());
	//printf("rgb_path:%s.length = %i\n",rgb_path.c_str(),rgb_path.length());
	//printf("depth_path:%s.length = %i\n",depth_path.c_str(),depth_path.length());
	long size = sizeof(id_number)+sizeof(timing)+rgb_path.length()+rgb_path.length()+depth_path.length();
	printf("total length: %ld\n",size);
	
	char * buffer_char 		= new char[size];
	int * buffer_int 		= (int *) buffer_char;
	double * buffer_double 	= (double *) buffer_char;
	
	buffer_double[0] 	= timing;
	buffer_int[2] 		= id_number;
	buffer_int[3]		=rgb_path.length();
	buffer_int[4]		=depth_path.length();
	for(int i = 0; i < rgb_path.length(); i++)
	{
		buffer_char[20+i]=rgb_path.at(i);
	}
	for(int i = 0; i < depth_path.length(); i++)
	{
		buffer_char[20+i+rgb_path.length()]=depth_path.at(i);
	}
	ofstream outfile (path.c_str(),ofstream::binary);
	outfile.write (buffer_char,size);
	outfile.close();
}
void RGBDFrame::load(string path, RGBDCamera * used_camera)
{
	camera = used_camera;
	//camera->print();
	printf("RGBDFrame load: %s\n",path.c_str());
	ifstream infile (path.c_str(),ifstream::binary);
	infile.seekg(0,ifstream::end);
	long size=infile.tellg();
	char * buffer_char 		= new char [size];
	int * buffer_int 		= (int *) buffer_char;
	double * buffer_double 	= (double *) buffer_char;
	infile.seekg(0);
	infile.read (buffer_char,size);
	infile.close();
	timing 				=buffer_double[0];
	id_number 			=buffer_int[2];
	int length_rgb 		=buffer_int[3];
	int length_depth 	=buffer_int[4];
	printf("timing = %f\n",timing);
	printf("id_number = %i\n",id_number);
	printf("lengths: %i and %i\n",length_rgb,length_depth);
	char * rgb_p 			= new char[length_rgb+1];
	rgb_p[length_rgb] 		= 0;
	char * depth_p 			= new char[length_depth+1];
	depth_p[length_depth] 	= 0;
	for(int i = 0; i < length_rgb; i++)
	{
		rgb_p[i] = buffer_char[20+i];
	}
	
	for(int i = 0; i < length_depth; i++)
	{
		depth_p[i] = buffer_char[20+i+length_rgb];
	}
	rgb_path = string(rgb_p);
	depth_path = string(depth_p);
	printf("imgs:%s\n%s\n",rgb_path.c_str(),depth_path.c_str());
	printf("__________________________\n");
	
	keypoints = 0;
	setupKeyPointData();
	setupImageDescriptor();
	showFrame();
	printf("frame loaded\n");
	//cvWaitKey(0);
}


void RGBDFrame::setupKeyPointData()
{

	print();
	float focal_length_x = camera->focal_length_x;
	float focal_length_y = camera->focal_length_y;
	float optical_center_x = camera->optical_center_x;
	float optical_center_y = camera->optical_center_y;
	float depth_scaling = camera->depth_scaling;
	float factor = camera->factor;	
	int width = camera->width;
	int height = camera->height;

	IplImage* img = cvLoadImage( rgb_path.c_str(), CV_LOAD_IMAGE_GRAYSCALE);
	IplImage* depth_img = cvLoadImage( depth_path.c_str(), CV_LOAD_IMAGE_UNCHANGED );
	/*if(camera->camera_type == 0)
	{
		depth_img = cvLoadImage( depth_path.c_str(), CV_LOAD_IMAGE_UNCHANGED );
	}
	else if(camera->camera_type == 1)
	{
		depth_img = cvLoadImage( depth_path.c_str(), CV_LOAD_IMAGE_GRAYSCALE);
	}
	*/
	IplImage* img_col = cvLoadImage( rgb_path.c_str(), 1);
	printf("-------------------\n");
	printf("%i\n",	depth_img->nSize);
    printf("%i\n", 	depth_img->ID);
    printf("%i\n",  depth_img->nChannels);
    printf("%i\n",  depth_img->alphaChannel);
    printf("%i\n",  depth_img->depth);
    printf("%i\n",  depth_img->dataOrder);
    printf("%i\n",  depth_img->origin);
    printf("%i\n",  depth_img->align);
    printf("%i\n",  depth_img->width);
    printf("%i\n",  depth_img->height);
    printf("-------------------\n");
	
	keypoints = new KeyPointSet();

	CvMemStorage* storage = cvCreateMemStorage(0);

	CvSeq *imageKeypoints = 0;
	CvSeq *imageDescriptors = 0;
	int found = 0;
	keypoints->stabilety_threshold = 1000;

    while(found < 500)
    {
	   	CvSURFParams params = cvSURFParams(keypoints->stabilety_threshold, 1);
		cvExtractSURF(img, 0, &imageKeypoints, &imageDescriptors, storage, params);
		found = imageDescriptors->total;
		keypoints->stabilety_threshold *= 0.5;
    }

	CvSeqReader reader, kreader;
	cvStartReadSeq( imageKeypoints, &kreader );
	cvStartReadSeq( imageDescriptors, &reader );
	vector<CvPoint2D32f> pt1, pt2;	
	int index_forward = 0;
	int index_backward = imageKeypoints->total - 1;
	int index;
	
	KeyPoint ** tmp_points = new KeyPoint*[found];
	int nr_valid = 0;
	int nr_invalid = 0;
/*
	for(int i = 0; i < found; i++ )
	{
		const CvSURFPoint* kp = (const CvSURFPoint*)kreader.ptr;
		const float* descriptor = (const float*)reader.ptr;
		CV_NEXT_SEQ_ELEM( kreader.seq->elem_size, kreader );
		CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
		
		tmp_points[i] 						= new KeyPoint();
		tmp_points[i]->frame_x 				= kp->pt.x;
		tmp_points[i]->frame_y 				= kp->pt.y;
		int x 								= round(tmp_points[i]->frame_x);
		int y 								= round(tmp_points[i]->frame_y);
		unsigned char d1 					= depth_img->imageData[3 * (y * depth_img->width + x) + 0];
		unsigned char d2 					= depth_img->imageData[3 * (y * depth_img->width + x) + 1];
		float z_pos 						= (d1*256 + d2)/2000.0;
		tmp_points[i]->stabilety 			= kp->hessian;
		tmp_points[i]->valid 				= z_pos > 0 && z_pos < 1.85;;
		tmp_points[i]->position(0)			= tan_angle_width*(x - mid_width) * z_pos / mid_width;
		tmp_points[i]->position(1)			= tan_angle_height*(y - mid_height) * z_pos / mid_height;
		tmp_points[i]->position(2)			= z_pos;
		tmp_points[i]->px					= tmp_points[i]->position(0);
		tmp_points[i]->py					= tmp_points[i]->position(1);
		tmp_points[i]->pz					= tmp_points[i]->position(2);
		tmp_points[i]->r					= img_col->imageData[3 * (y * img_col->width + x) + 2];
		tmp_points[i]->g					= img_col->imageData[3 * (y * img_col->width + x) + 1];
		tmp_points[i]->b					= img_col->imageData[3 * (y * img_col->width + x) + 0];
		tmp_points[i]->surf_descriptor 		= new FeatureDescriptor(128);
		for(int j = 0; j < 128; j++){tmp_points[i]->surf_descriptor->descriptor[j] = descriptor[j];}
		tmp_points[i]->surf_descriptor->laplacian = kp->laplacian;
		tmp_points[i]->surf_descriptor->type = 1;
		if(tmp_points[i]->valid){nr_valid++;}
		else{nr_invalid++;}
	}
*/
	keypoints->nr_valid_points		= nr_valid;
	keypoints->valid_key_points 	= new KeyPoint*[nr_valid];
	
	keypoints->nr_invalid_points	= nr_invalid;
	keypoints->invalid_key_points 	= new KeyPoint*[nr_invalid];
	
	int count_valid = 0;
	int count_invalid = 0;
	
	for(int i = 0; i < found; i++ )
	{
		if(tmp_points[i]->valid){
			keypoints->valid_key_points[count_valid] 					= tmp_points[i];
			keypoints->valid_key_points[count_valid]->index_number 		= count_valid;
			count_valid++;
		}else{
			keypoints->invalid_key_points[count_invalid] 				= tmp_points[i];
			keypoints->invalid_key_points[count_invalid]->index_number 	= count_invalid;
			count_invalid++;
		}
	}
	
    delete tmp_points;
    
    keypoints->sortKeyPoints();
   
	cvReleaseImage( &img);
	cvReleaseImage( &img_col);
	cvReleaseImage( &depth_img);

	cvReleaseMemStorage(&storage);
//	cvRelease((void **)&imageKeypoints);
//	cvRelease((void **)&imageDescriptors);


	pcl::PointCloud<pcl::PointXYZRGB>::Ptr surf_cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
	surf_cloud->width = keypoints->nr_valid_points;
	surf_cloud->height = 1;
	surf_cloud->is_dense = false;
	surf_cloud->points.resize(keypoints->nr_valid_points);
	for (int i = 0; i < keypoints->nr_valid_points; i++)
	{
		surf_cloud->points[i].x = keypoints->valid_key_points[i]->position(0);
		surf_cloud->points[i].y = keypoints->valid_key_points[i]->position(1);
		surf_cloud->points[i].z = keypoints->valid_key_points[i]->position(2);
		surf_cloud->points[i].r = keypoints->valid_key_points[i]->r;
		surf_cloud->points[i].g = keypoints->valid_key_points[i]->g;
		surf_cloud->points[i].b = keypoints->valid_key_points[i]->b;
	}
	
	keypoint_cloud = surf_cloud;
	
	//pcl::visualization::CloudViewer viewer("Cloud Viewer_2");
	//viewer.showCloud(keypoint_cloud);
	//cvWaitKey(0);	
}

pcl::PointCloud<pcl::PointXYZRGB>::Ptr RGBDFrame::getCloud()
{
/*
	printf("get cloud\n");
	double tan_angle_width 		= tan(camera->angle_width);
	double tan_angle_height		= tan(camera->angle_height);
	double width_d 				= camera->width;
	double height_d 			= camera->height;
	double mid_width 			= camera->mid_width;
	double mid_height 			= camera->mid_height;
	
	IplImage* depth_img = cvLoadImage( depth_path.c_str(), CV_LOAD_IMAGE_COLOR );
	IplImage* img_col = cvLoadImage( rgb_path.c_str(), 1);
	int nr_particles = 0;
	for(int x = 0; x < img_col->width; x++ )
	{
		for(int y = 0; y < img_col->height; y++ )
		{
			unsigned char d1		= depth_img->imageData[3 * (y * depth_img->width + x) + 0];
			unsigned char d2 		= depth_img->imageData[3 * (y * depth_img->width + x) + 1];
			float z_pos 			= (d1*256 + d2)/2000.0;
			if( z_pos > 0 && z_pos < 1.85){nr_particles++;}
		}
	}

	
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp_cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
	tmp_cloud->width = nr_particles;
	tmp_cloud->height = 1;
	tmp_cloud->is_dense = false;
	tmp_cloud->points.resize(nr_particles);
	
	int i = 0;
	for(int x = 0; x < img_col->width; x++)
	{
		for(int y = 0; y < img_col->height; y++)
		{
			unsigned char d1		= depth_img->imageData[3 * (y * depth_img->width + x) + 0];
			unsigned char d2 		= depth_img->imageData[3 * (y * depth_img->width + x) + 1];
			float z_pos 			= (d1*256 + d2)/2000.0;
			if( z_pos > 0 && z_pos < 1.85){			
				tmp_cloud->points[i].x = tan_angle_width*(x - mid_width) * z_pos / mid_width;
				tmp_cloud->points[i].y = tan_angle_height*(y - mid_height) * z_pos / mid_height;
				tmp_cloud->points[i].z = z_pos;
				tmp_cloud->points[i].r = img_col->imageData[3 * (y * img_col->width + x) + 2];
				tmp_cloud->points[i].g = img_col->imageData[3 * (y * img_col->width + x) + 1];
				tmp_cloud->points[i].b = img_col->imageData[3 * (y * img_col->width + x) + 0];
				i++;
			}
		}
	}
	
	cvReleaseImage( &img_col);
	cvReleaseImage( &depth_img);
	printf("done getting cloud\n");
	*/
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp_cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
	return tmp_cloud;

}

const bool debugg_setupImageDescriptor = false;
void RGBDFrame::setupImageDescriptor()
{
/*
    IplImage * rgb_img;
	if(debugg_setupImageDescriptor){rgb_img = cvLoadImage( rgb_path.c_str(), CV_LOAD_IMAGE_COLOR );}
	
	FeatureDescriptor ** bags 	= camera->words;
	int nr_words 				= camera->nr_words;
	image_descriptor 			= new FeatureDescriptor(nr_words);
	image_descriptor->type		= 0;
//exit(0);
	for(unsigned int i = 0; i < keypoints->nr_valid_points; i++)
	{
		if(debugg_setupImageDescriptor){
			cvCircle(rgb_img,cvPoint(keypoints->valid_key_points[i]->frame_x, keypoints->valid_key_points[i]->frame_y), 5,cvScalar(0, 0, 255, 0),2, 8, 0);
		}
		float best_value = 99999999;
		int best_index = -1;
		float * word_distances = new float[nr_words];
		for(unsigned int j = 0; j < nr_words; j++)
		{
			double value = keypoints->valid_key_points[i]->surf_descriptor->distance(bags[j]);
			word_distances[j] = value;
			if(value < best_value){
				best_index = j;
				best_value = value;
			}
		}
		keypoints->valid_key_points[i]->setWordDistances(word_distances, nr_words);
		image_descriptor->descriptor[best_index]++;
	}
	//cvWaitKey(0);
	for(unsigned int i = 0; i < keypoints->nr_invalid_points; i++)
	{
		if(debugg_setupImageDescriptor){
			cvCircle(rgb_img,cvPoint(keypoints->invalid_key_points[i]->frame_x, keypoints->invalid_key_points[i]->frame_y), 5,cvScalar(0, 0, 255, 0),2, 8, 0);
		}
		float best_value = 99999999;
		int best_index = -1;
		float * word_distances = new float[nr_words]; 
		for(unsigned int j = 0; j < nr_words; j++)
		{
			double value = keypoints->invalid_key_points[i]->surf_descriptor->distance(bags[j]);
			word_distances[j] = (float)value;
			if(value < best_value){
				best_index = j;
				best_value = value;
			}
		}
		keypoints->invalid_key_points[i]->setWordDistances(word_distances, nr_words);
		//printf("best: %i,%f <---> %i,%f\n",best_index,best_value,keypoints->invalid_key_points[i]->word_distances_sort_index[0],keypoints->invalid_key_points[i]->word_distances_sort_value[0]);
		image_descriptor->descriptor[best_index]++;
	}
    if(debugg_setupImageDescriptor){
		cvNamedWindow("rgb_pixel_image", CV_WINDOW_AUTOSIZE );
		cvShowImage("rgb_pixel_image", rgb_img);
		cvWaitKey(0);
		cvReleaseImage( &rgb_img );
    }
	for(unsigned int i = 0; i < nr_words; i++)
	{
		image_descriptor->descriptor[i] /= (float)(keypoints->nr_invalid_points + keypoints->nr_valid_points);
		image_descriptor->descriptor[i] *= (float)(nr_words);
	}
	
	
	cluster_keypoints 			= new KeyPointSet*[nr_words];
	nr_clusters 				= nr_words;
	int * nr_valid_in_cluster 	= new int[nr_clusters];
	int * nr_invalid_in_cluster	= new int[nr_clusters];
	for(int i = 0; i < nr_clusters; i++)
	{
		cluster_keypoints[i] = new KeyPointSet();
		nr_valid_in_cluster[i] = 0;
		nr_invalid_in_cluster[i] = 0;
	}
	for(unsigned int i = 0; i < keypoints->nr_valid_points; i++)
	{
		//KeyPoint * kp = keypoints->valid_key_points[i];
		//int index = kp->word_distances_sort_index[0];
		//printf("index: %i\n",index);
		nr_valid_in_cluster[keypoints->valid_key_points[i]->word_distances_sort_index[0]]++;
	}
	for(unsigned int i = 0; i < keypoints->nr_invalid_points; i++)
	{
		nr_invalid_in_cluster[keypoints->invalid_key_points[i]->word_distances_sort_index[0]]++;
	}
	
	for(int i = 0; i < nr_clusters; i++)
	{
		cluster_keypoints[i]->valid_key_points 		= new KeyPoint*[nr_valid_in_cluster[i]];
		cluster_keypoints[i]->invalid_key_points 	= new KeyPoint*[nr_invalid_in_cluster[i]];
		cluster_keypoints[i]->nr_valid_points 		= 0;//nr_valid_in_cluster[i];
		cluster_keypoints[i]->nr_invalid_points 	= 0;//nr_invalid_in_cluster[i];    
	}
	
	for(unsigned int i = 0; i < keypoints->nr_valid_points; i++)
	{
		int index = keypoints->valid_key_points[i]->word_distances_sort_index[0];
		int added = cluster_keypoints[index]->nr_valid_points;
		cluster_keypoints[index]->valid_key_points[added] = keypoints->valid_key_points[i];
		cluster_keypoints[index]->nr_valid_points++;
	}
	for(unsigned int i = 0; i < keypoints->nr_invalid_points; i++)
	{
		int index = keypoints->invalid_key_points[i]->word_distances_sort_index[0];
		int added = cluster_keypoints[index]->nr_invalid_points;
		cluster_keypoints[index]->invalid_key_points[added] = keypoints->invalid_key_points[i];
		cluster_keypoints[index]->nr_invalid_points++;
	}
	
	for(int i = 0; i < nr_clusters; i++)
	{
		cluster_keypoints[i]->sortKeyPoints();
	}
	delete nr_valid_in_cluster;
	delete nr_invalid_in_cluster;
	//exit(0);
	*/
}

void RGBDFrame::showFrame()
{

	printf("show frame!\n");
    IplImage * rgb_img = cvLoadImage( rgb_path.c_str(), CV_LOAD_IMAGE_COLOR );
    IplImage * depth_img = cvLoadImage( depth_path.c_str(), CV_LOAD_IMAGE_COLOR );

	cvNamedWindow("rgb_pixel_image", CV_WINDOW_AUTOSIZE );
	cvShowImage("rgb_pixel_image", rgb_img);
	cvNamedWindow("depth_pixel_image", CV_WINDOW_AUTOSIZE );
	cvShowImage("depth_pixel_image", depth_img);
	
    cvWaitKey(1000/30);
    cvReleaseImage( &rgb_img );
	cvReleaseImage( &depth_img );
}

double RGBDFrame::compareToFrame(RGBDFrame * frame)
{
	return sqrt(image_descriptor->distance(frame->image_descriptor));
}

RGBDFrame * RGBDFrame::clone()
{

	printf("cloned:: May be bugged\n");
	RGBDFrame * new_frame 	= new RGBDFrame();
	new_frame->camera 		= camera;
	new_frame->id_number	= id_number;
	new_frame->rgb_path 	= rgb_path;
	new_frame->depth_path 	= depth_path;
	new_frame->timing 		= timing;
	new_frame->setupKeyPointData();
	return new_frame;
}
